#include "smc.h"
#include <math.h>

void smc_controller_create(smc_controller* smc,float k,float FILTER_ALPHA,float exponent,int32_t rate,float output_max)
{
        smc->k=k;
    smc->FILTER_ALPHA=FILTER_ALPHA;
        if (rate == 0) 
        {
                rate = 1;
        }
        smc->rate=rate;
        smc->last_output=0;
        smc->output_max=output_max;
    smc->filtered_output=0;
        smc->exponent=exponent;
        smc->output=0;
}

void smc_calculate(smc_controller * smc)
{
        smc -> integral += smc->recent_error;
        if(smc->integral>=20000)
        {
                smc->integral=20000;
        }
        if(smc->integral<=-20000)
        {
                smc->integral=-20000;
        }
        smc->s=smc->recent_error+smc->integral;
        if(smc->s>0)
        {
                smc->output=smc->k*pow(fabs(smc->s),smc->exponent);
        }
        if(smc->s==0)
        {
                smc->output=0;
        }
        if(smc->s<0)
        {
                smc->output=-smc->k*pow(fabs(smc->s),smc->exponent);
        }
    smc->filtered_output = smc->FILTER_ALPHA * smc->last_output + (1 - smc->FILTER_ALPHA) * smc->output;
        if(smc->filtered_output > smc->output_max)smc->filtered_output = smc->output_max;
        else if(smc->filtered_output < - smc->output_max)smc->filtered_output = - smc->output_max;
        smc->last_output=smc->filtered_output;
}

void smc_error_input(smc_controller * smc, float error)
{
        smc -> recent_error = error;
        smc_calculate(smc);
}

float smc_output(smc_controller * smc)
{
        return smc->filtered_output;
}